/* BFS、DFS、AStar */

#include <stdio.h>
#include <stdlib.h>
// #include "My-Stack.c"
#include "my_priority_queue.c"

/**
 * @brief 计算曼哈顿距离
 * @return 两坐标的曼哈顿距离
*/
int manhattanDistance(int x1, int y1, int x2, int y2) {
    return abs(x1 - x2) + abs(y1 - y2);
}

/**
 * @brief 传入迷宫(起点终点), 返回路径
 * @param maze 规定：5为通路, 6为墙壁, 7为已走过
 * @return stack[][], stack[0]为整个搜索路径, stack[1]为一条到终点的路径
*/
stack** solveMazeByDFS(int** maze, int mazeRows, int mazeCols, int startX, int startY, int endX, int endY) {
    int* now;
    int nowX, nowY, nextX, nextY, x, y, i;
    stack* path = createStack(); // 整个搜索路径
    stack* s = createStack(); // 一条到终点的路径
    stack** twoPath = (stack**)malloc(2 * sizeof(stack*));
    twoPath[0] = path, twoPath[1] = s;
    int direction[4][2] = {{1,0},{-1,0},{0,1},{0,-1}}; // 方位(上下左右)
    int pd = 0; // 判断四个方位能不能走
    appendToStack(s, startX, startY); // 起点入栈

    while (!ifEmpty(s)) {
        now = getTopInStack(s); nowX = now[0], nowY = now[1];
        appendToStack(path, nowX, nowY); // 记录整个搜索路径
        if (nowX == endX && nowY == endY) { // 判断是否到达终点
            return twoPath;
        }
        maze[nowX][nowY] = 7; // 标记已走过

        // 判断哪里能走
        for (i = 0; i < 4; i++) {
            x = direction[i][0], y = direction[i][1];
            nextX = nowX + x, nextY = nowY + y;
            if (mazeRows > nextX  && nextX > -1 && mazeCols > nextY && nextY > -1 && maze[nextX][nextY] == 5) {
                appendToStack(s, nextX, nextY);
                pd = 1; break;
            }
        }
        if (!pd) { // 代表四个方位都不能走
            popToStack(s);
        } else {
            pd = 0;
        }
    }
    return twoPath;
}

stack** solveMazeByBFS(int** maze, int mazeRows, int mazeCols, int startX, int startY, int endX, int endY) {
    int* now;
    int nowX, nowY, nextX, nextY, x, y, i;
    int pre[mazeRows][mazeCols][2]; // 哈希表, 用于记录“前驱结点”
    stack** twoPath = (stack**)malloc(2 * sizeof(stack*));
    stack* path = createStack();
    stack* minPath = createStack();
    twoPath[0] = path, twoPath[1] = minPath;
    stack* deque = createStack();
    appendToStack(deque, startX, startY); // 起点入队列
    maze[startX][startY] = 7;
    int direction[4][2] = {{1,0},{-1,0},{0,1},{0,-1}}; // 方位(上下左右)
    int pd = 0; // 判断是否走出迷宫

    while (!ifEmpty(deque)) {
        now = removeToDynamicArray(deque->array, 0);
        deque->top--;
        nowX = now[0], nowY = now[1];
        appendToStack(path, nowX, nowY); // 记录整个搜索路径
        if (nowX == endX && nowY == endY) {
            pd = 1;
            break;
        }

        // 判断哪里能走
        for (i = 0; i < 4; i++) {
            x = direction[i][0], y = direction[i][1];
            nextX = nowX + x, nextY = nowY + y;
            if (mazeRows > nextX  && nextX > -1 && mazeCols > nextY && nextY > -1 && maze[nextX][nextY] == 5) {
                appendToStack(deque, nextX, nextY);
                pre[nextX][nextY][0] = nowX, pre[nextX][nextY][1] = nowY;
                maze[nextX][nextY] = 7; // 标记已走过
            }
        }
    }
    if (pd) {
        // 如果走出了迷宫,则生成最短路径
        appendToStack(minPath, endX, endY);
        int x = endX, y = endY, temp1, temp2;
        while (x != startX || y != startY) {
            temp1 = x, temp2 = y;
            x = pre[temp1][temp2][0], y = pre[temp1][temp2][1];
            appendToStack(minPath, x, y);
        }
    }
    return twoPath;
}

stack** solveMazeByAStar(int** maze, int mazeRows, int mazeCols, int startX, int startY, int endX, int endY) {
    MinHeap* priorityQueue = createMinHeap(sizeof(node*)); // 优先级队列
    offer(priorityQueue, createNode(startX, startY, 0)); // 起点入队列
    stack** twoPath = (stack**)malloc(2 * sizeof(stack*)); // 储存两个路径
    stack* path = createStack(); // 记录所有搜索路径
    stack* minPath = createStack(); // 记录最短路径
    twoPath[0] = path, twoPath[1] = minPath;
    int pre[mazeRows][mazeCols][2]; // 哈希表(记录前驱)

    node* nowNode; int* now;
    int nowX, nowY, nextX, nextY, newCost, priority, i, x, y; 
    int direction[4][2] = {{1,0},{-1,0},{0,1},{0,-1}}; // 方位(上下左右)
    int pd = 0; // 判断是否走出迷宫

    /* 初始化为-1, -1代表还未搜索过 */
    int costSoFar[mazeRows][mazeCols]; // 哈希表(记录代价)
    int i1, i2;
    for (i1 = 0; i1 < mazeRows; i1++) {
        for (i2 = 0; i2 < mazeCols; i2++) {
            costSoFar[i1][i2] = -1;
        }
    }
    costSoFar[startX][startY] = 0;

    while (!ifEmptyInHeap(priorityQueue)) {
        nowNode = poll(priorityQueue, 0); // 弹出队首元素
        now = nowNode->data;
        nowX = now[0], nowY = now[1];
        appendToStack(path, nowX, nowY); // 用于记录整个搜索路径
        if (nowX == endX && nowY == endY) {
            pd = 1;
            break;
        }
        for (i = 0; i < 4; i++) {
            x = direction[i][0], y = direction[i][1];
            nextX = nowX + x, nextY = nowY + y;
            if (nextX > -1 && nextY > -1 && nextX < mazeRows && nextY < mazeCols && maze[nextX][nextY] == 5) {
                newCost = costSoFar[nowX][nowY] + 1;
                if (costSoFar[nextX][nextY] == -1 || newCost < costSoFar[nextX][nextY]) {
                    costSoFar[nextX][nextY] = newCost;
                    priority = newCost + manhattanDistance(endX, endY, nextX, nextY);
                    offer(priorityQueue ,createNode(nextX, nextY, priority));
                    pre[nextX][nextY][0] = nowX, pre[nextX][nextY][1] = nowY;
                }
            }
        }
    }
    if (pd) {
        // 如果走出了迷宫,则生成最短路径
        appendToStack(minPath, endX, endY);
        int x = endX, y = endY, temp1, temp2;
        while (x != startX || y != startY) {
            temp1 = x, temp2 = y;
            x = pre[temp1][temp2][0], y = pre[temp1][temp2][1];
            appendToStack(minPath, x, y);
        }
    }
    return twoPath;
}

void main() {
    int rows = 17;
    int cols = 17;

    // 分配给指针数组的指针
    int** maze = (int**)malloc(rows * sizeof(int*));

    // 分配每一行的内存，并将指针存储在指针数组中
    for (int i = 0; i < rows; i++) {
        maze[i] = (int*)malloc(cols * sizeof(int));
    }

    // 初始化二维数组
    int mazeData[17][17] = {{5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5},
    {5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5},
    {5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5},
    {5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5},
    {5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5},
    {5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5},
    {5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5},
    {5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5},
    {5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5},
    {5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5},
    {5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5},
    {5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5},
    {5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5},
    {5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5},
    {5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5},
    {5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5},
    {5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5}
    };

    // 将数据从二维数组复制到指针数组
    for (int i = 0; i < rows; i++) {
        for (int j = 0; j < cols; j++) {
            maze[i][j] = mazeData[i][j];
        }
    }
    stack* s = solveMazeByAStar(maze, rows, cols, 0, 0, 16, 16)[1];
    int i;
    int* aa;
    for (i = 0; i < s->top + 1; i++) {
        aa = (int*) s->array->array[i];
        printf("x:%d y:%d\n", aa[0], aa[1]);
    }
}
